#include "model/ears_model.h"

void EarsModel::init() {
	emotion_handler_sub_ = nh_.subscribe(Topic::EMO_HANDLER_EARS, 1, &EarsModel::handlerCallback, this);

	joint_state_.name.resize(2);
	joint_state_.position.resize(2);

	joint_state_.name[0] = Joint::LEFTEAR;
	joint_state_.name[1] = Joint::RIGHTEAR;
}

void EarsModel::spin() {
	ros::Rate loop_rate(30);
	setInPosition(true);

	while (ros::ok()) {
		if (!isInPosition()) {
			moveEars();
		}

		prepState();
		joint_pub_.publish(joint_state_);
		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Handler Callback
 * Receives which position the ears have to be in.
 *
 * @param		state	Int32, 0 = NEUTRAL, 1 = FRONT, 2 = REAR.
 */
void EarsModel::handlerCallback(const comm::ears::ConstPtr &state) {
	int lstate = state->leftstate;
	int rstate = state->rightstate;

	if (lposition_ != lstate || rposition_ != rstate) {
		lposition_ = state->leftstate;
		rposition_ = state->rightstate;
		setInPosition(false);
	}

	if (lstate == EARS_NEUTRAL) {
		langle_ = state->leftangle;
	}
	if (rstate == EARS_NEUTRAL) {
		rangle_ = state->rightangle;
	}
}

void EarsModel::moveEars() {
	if (lposition_ == EARS_REAR) {
		if (langle_ > ModelValues::EARS_MIN_ROTATION) {
			langle_ -= ModelValues::EARS_STEP;
		}
	} else if (lposition_ == EARS_FRONT) {
		if (langle_ < ModelValues::EARS_MAX_ROTATION) {
			langle_ += ModelValues::EARS_STEP;
		}
	} else if (lposition_ == EARS_FLAP){
		if (langle_ >= ModelValues::EARS_MAX_ROTATION) {
			lflapper_ = -1;
		} else if (langle_ <= ModelValues::EARS_MIN_ROTATION) {
			lflapper_ = 1;
		}
		langle_ += ModelValues::EARS_STEP * lflapper_;
	}

	if (rposition_ == EARS_REAR) {
		if (rangle_ > ModelValues::EARS_MIN_ROTATION) {
			rangle_ -= ModelValues::EARS_STEP;
		}
	} else if (rposition_ == EARS_FRONT) {
		if (rangle_ < ModelValues::EARS_MAX_ROTATION) {
			rangle_ += ModelValues::EARS_STEP;
		}
	} else if (rposition_ == EARS_FLAP){
		if (rangle_ >= ModelValues::EARS_MAX_ROTATION) {
			rflapper_ = -1;
		} else if (rangle_ <= ModelValues::EARS_MIN_ROTATION) {
			rflapper_ = 1;
		}
		rangle_ += ModelValues::EARS_STEP * rflapper_;
	}
}

void EarsModel::prepState() {
	joint_state_.header.stamp = ros::Time::now();
	joint_state_.position[0] = langle_;
	joint_state_.position[1] = rangle_;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "ears_model_publisher");

	EarsModel ears;
	ears.init();
	ears.spin();
}
